package algo.astar;

import algo.Node;
import algo.dijkstra.Transformer;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Set;
import java.util.Stack;

/**
 *
 * @author robin
 */
public class AStarAlgorithm {

    private Node start, destination;
    private int[][][] graph;
    private long algorithmTime;

    public AStarAlgorithm(int[][][] graph, Node start, Node destination) {
        this.start = start;
        start.isStart();
        this.destination = destination;
        this.graph = graph;
    }

    public List<Node> searchPath() {
        long startTime = System.currentTimeMillis();
        Map<Integer, AStarNode> openSet = new HashMap<Integer, AStarNode>();
        Map<Integer, AStarNode> closeSet = new HashMap<Integer, AStarNode>();
        PriorityQueue<AStarNode> pQueue = new PriorityQueue<AStarNode>(20, new AStarNodeComparator());

        AStarNode aStart = new AStarNode(start, 0, calcManhattenDistance(start.block, destination.block));
        openSet.put(aStart.getBlock(), aStart);
        pQueue.add(aStart);

        AStarNode goal = null;
        while (openSet.size() > 0) {
            AStarNode x = pQueue.poll();
            openSet.remove(x.getBlock());

            if (x.getNode().equals(destination)) {
                goal = x;
                break;
            } else {
                closeSet.put(x.getBlock(), x);
                CreateGraph(openSet, closeSet, pQueue, x);
            }
        }

        if (goal != null) {
            return CreatePath(goal, startTime);
        } else {
            return null;
        }
    }

    public long getAlgorithmTime() {
        return algorithmTime;
    }

    private double calcManhattenDistance(int srcBlock, Integer tgBlock) {
        int[] source = Transformer.transformGraphPointTo2D(srcBlock);
        int[] target = Transformer.transformGraphPointTo2D(tgBlock);

        double dx = Math.abs(source[0] * 10 - target[0] * 10);
        double dy = Math.abs(source[1] * 10 - target[1] * 10);

        return dx + dy;
    }

    private Set<Node> getNeighbors(Node node) {
        Set<Node> neighbors = new HashSet<Node>();
        int[] couple;
        couple = Transformer.transformGraphPointTo2D(node.block);

        //N
        if (graph[couple[0]][couple[1]][0] == 0) {
            int x = couple[0];
            int y = couple[1] - 1;

            int[] aCouple = {x, y};

            neighbors.add(new Node(Transformer.transformCoupleToBlock(aCouple)));
        }
        //E
        if (graph[couple[0]][couple[1]][1] == 0) {
            int x = couple[0] + 1;
            int y = couple[1];
            int[] aCouple = {x, y};
            neighbors.add(new Node(Transformer.transformCoupleToBlock(aCouple)));
        }
        //S
        if (graph[couple[0]][couple[1]][2] == 0) {
            int x = couple[0];
            int y = couple[1] + 1;

            int[] aCouple = {x, y};

            neighbors.add(new Node(Transformer.transformCoupleToBlock(aCouple)));
        }
        //W
        if (graph[couple[0]][couple[1]][3] == 0) {
            int x = couple[0] - 1;
            int y = couple[1];
            int[] aCouple = {x, y};
            neighbors.add(new Node(Transformer.transformCoupleToBlock(aCouple)));
        }
        return neighbors;
    }

    private List<Node> CreatePath(AStarNode goal, long startTime) {
        List<Node> list = new ArrayList<Node>();
        list.add(goal.getNode());
        AStarNode parent = goal.getParent();

        while (parent != null) {
            list.add(parent.getNode());
            parent = parent.getParent();
        }
        Collections.reverse(list);

        long stopTime = System.currentTimeMillis();
        algorithmTime = stopTime - startTime;
        return list;
    }

    private void CreateGraph(Map<Integer, AStarNode> openSet, Map<Integer, AStarNode> closeSet, PriorityQueue<AStarNode> pQueue, AStarNode x) {
        Set<Node> neighbors = getNeighbors(x.getNode());
        
        for (Node neighbor : neighbors) {
            AStarNode visited = closeSet.get(neighbor.block);
            if (visited == null) {
                double g = x.getG() + calcManhattenDistance(x.getBlock(), neighbor.block);

                AStarNode n = openSet.get(neighbor.block);
                if (n == null) {
                    n = new AStarNode(neighbor, g, calcManhattenDistance(neighbor.block, destination.block));
                    n.setParent(x);
                    openSet.put(n.getBlock(), n);
                    pQueue.add(n);
                } else if (g < n.getG()) {
                    //found better route
                    n.setParent(x);
                    n.setG(g);
                    n.setH(calcManhattenDistance(neighbor.block, destination.block));
                }
            }
        }
    }
}